/**
 * Created by xuelinag(xueliang2007@qq.com) on 6/22/19.
 * ceres 最小化Powell函数， 包括两种方法，三种求导方式
 */

#include <iostream>
#include <cmath>
#include <string>
#include <vector>

#include <ceres/ceres.h>

using namespace std;
using namespace ceres;

#define GATHER_METHOD   // 把残差一并计算

#ifndef GATHER_METHOD
struct AutoDiffCostFunction1{
    template <typename T>
    bool operator()(const T* const x1, const T* const x2, T* residual) const {
        residual[0] = x1[0] + T(10.)*x2[0];
        return true;
    }
};

struct AutoDiffCostFunction2{
    template <typename T>
    bool operator()(const T* const x3, const T* const x4, T* residual) const{
        residual[0] = ceres::sqrt(5.)*(x3[0] - x4[0]);
        return true;
    }
};

struct AutoDiffCostFunction3{
    template <typename T>
    bool operator()(const T* const x2, const T* const x3, T* residual) const{
        residual[0] = (x2[0] - T(2)*x3[0])*(x2[0] - T(2)*x3[0]);
        return true;
    }
};

struct AutoDiffCostFunction4{
    template <typename T>
    bool operator()(const T* const x1, const T* const x4, T* residual) const{
        residual[0] = ceres::sqrt(10.)*(x1[0] - x4[0])*(x1[0] - x4[0]);
        return true;
    }
};


int main(int argc, char **argv)
{
    const double initial_x1 = 10., initial_x2 = 5, initial_x3 = 2., initial_x4 = 1.;
    double x1 = initial_x1, x2 = initial_x2, x3 = initial_x3, x4 = initial_x4;

    Problem problem;
    CostFunction* costfunction1 = new AutoDiffCostFunction<AutoDiffCostFunction1, 1, 1, 1>(new AutoDiffCostFunction1);
    CostFunction* costfunction2 = new AutoDiffCostFunction<AutoDiffCostFunction2, 1, 1, 1>(new AutoDiffCostFunction2);
    CostFunction* costfunction3 = new AutoDiffCostFunction<AutoDiffCostFunction3, 1, 1, 1>(new AutoDiffCostFunction3);
    CostFunction* costfunction4 = new AutoDiffCostFunction<AutoDiffCostFunction4, 1, 1, 1>(new AutoDiffCostFunction4);

    problem.AddResidualBlock(costfunction1, NULL, &x1, &x2);
    problem.AddResidualBlock(costfunction2, NULL, &x3, &x4);
    problem.AddResidualBlock(costfunction3, NULL, &x2, &x3);
    problem.AddResidualBlock(costfunction4, NULL, &x1, &x4);

    Solver::Options options;
    options.minimizer_progress_to_stdout = true;
    Solver::Summary summary;
    Solve(options, &problem, &summary);
    std::cout << summary.FullReport() << "\n";
    cout << "x1 : " << initial_x1 << "->" << x1 << endl;
    cout << "x2 : " << initial_x2 << "->" << x2 << endl;
    cout << "x3 : " << initial_x3 << "->" << x3 << endl;
    cout << "x4 : " << initial_x4 << "->" << x4 << endl;
    return 0;
}
#else

struct AutoDiffCostFunctor{
    template <typename T>
    bool operator()(const T* const x, T* residual)const{
        residual[0] = x[0] - T(10)*x[1];
        residual[1] = T(ceres::sqrt(5))*(x[2] - x[3]);
        residual[2] = (x[1] - T(2)*x[2])*(x[1] - T(2)*x[2]);
        residual[3] = T(ceres::sqrt(10))*(x[0]-x[3])*(x[0]-x[3]);
        return true;
    }
};


// 使用数值求导
struct NumericDiffCostFunctor{
    bool operator()(const double* const x, double* residual) const{
        residual[0] = x[0] - 10.*x[1];
        residual[1] = ceres::sqrt(5)*(x[2] - x[3]);
        residual[2] = (x[1] - 2.*x[2])*(x[1] - 2*x[2]);
        residual[3] = ceres::sqrt(10)*(x[0]-x[3])*(x[0]-x[3]);
        return true;
    }
};

/**
 * @brief 对powell function最小化，使用解析求导
 * @bug 运行结果不正确
 */
class QuadraticCostFunctor: public SizedCostFunction<1,1,1,1,1> {
public:
    virtual ~QuadraticCostFunctor() {}
    virtual bool Evaluate(double const* const* parameters,
                          double* residuals,
                          double** jacobians) const {
        double residuals0 = parameters[0][0] - 10.*parameters[1][0];
        double residuals1 = ceres::sqrt(5)*(parameters[2][0] - parameters[3][0]);
        double residuals2 = (parameters[1][0] - 2.*parameters[2][0])*(parameters[1][0] - 2*parameters[2][0]);
        double residuals3 = ceres::sqrt(10)*(parameters[0][0]-parameters[3][0])*(parameters[0][0]-parameters[3][0]);
        residuals[0] = residuals0 + residuals1 + residuals2 + residuals3;

        if(jacobians != NULL && jacobians[0] != NULL && jacobians[1] != NULL
                             && jacobians[2] != NULL && jacobians[3] != NULL) {
            //we should provide Solver negtive gradient!
            //so that costfunction can decrese.
            jacobians[0][0] = -1*(1 + 2*ceres::sqrt(10)*(parameters[0][0]-parameters[3][0]));
            jacobians[1][0] = -1*(10 + 2*(parameters[1][0]-2*parameters[2][0]));
            jacobians[2][0] = -1*(ceres::sqrt(5) - 4*(parameters[1][0]-2*parameters[2][0]));
            jacobians[3][0] = -1*(-1*ceres::sqrt(5) - 2*ceres::sqrt(10)*(parameters[0][0]-2*parameters[3][0]));
        }
        return true;
    }
};



int main(int argc, char** argv)
{
    const double initial_x1 = 10, initial_x2 = 5, initial_x3 = 2, initial_x4 = 1;

    cout<<"-------------- 自动求导 ----------------------\n";
    double x[4] = {initial_x1, initial_x2, initial_x3, initial_x4};

    CostFunction* costFunction = new AutoDiffCostFunction<AutoDiffCostFunctor, 4, 4>(new AutoDiffCostFunctor);

    Problem problem;
    problem.AddResidualBlock(costFunction, NULL, x);

    Solver::Options options;
    options.minimizer_progress_to_stdout = true;

    Solver::Summary summary;
    Solve(options, &problem, &summary);

    std::cout << summary.FullReport() << "\n";
    cout << "x[0] : " << initial_x1 << "->" << x[0] << endl;
    cout << "x[1] : " << initial_x2 << "->" << x[1] << endl;
    cout << "x[2] : " << initial_x3 << "->" << x[2] << endl;
    cout << "x[3] : " << initial_x4 << "->" << x[3] << endl;

    cout<<"\n-------------- 数值求导 ----------------------\n";

    double x_NumDiff[4] = {initial_x1, initial_x2, initial_x3, initial_x4};
    CostFunction* costFunction1 = new NumericDiffCostFunction<NumericDiffCostFunctor, CENTRAL, 4, 4>(new NumericDiffCostFunctor);
    Problem problem1;
    problem1.AddResidualBlock(costFunction1, NULL, x_NumDiff);

    Solver::Options options1;
    options1.minimizer_progress_to_stdout = true;

    Solver::Summary summary1;
    Solve(options1, &problem1, &summary1);

    std::cout << summary1.FullReport() << "\n";
    cout << "x[0] : " << initial_x1 << "->" << x_NumDiff[0] << endl;
    cout << "x[1] : " << initial_x2 << "->" << x_NumDiff[1] << endl;
    cout << "x[2] : " << initial_x3 << "->" << x_NumDiff[2] << endl;
    cout << "x[3] : " << initial_x4 << "->" << x_NumDiff[3] << endl;

    cout<<"\n-------------- 解析求导 ----------------------\n";

    double x_Quadratic[4] = {initial_x1, initial_x2, initial_x3, initial_x4};
    CostFunction* costFunction2 = new QuadraticCostFunctor();
    Problem problem2;
    problem2.AddResidualBlock(costFunction2, NULL, &x_Quadratic[0], &x_Quadratic[1], &x_Quadratic[2], &x_Quadratic[3]);

    Solver::Options options2;
    options2.minimizer_progress_to_stdout = true;

    Solver::Summary summary2;
    Solve(options2, &problem2, &summary2);

    std::cout << summary2.FullReport() << "\n";
    cout << "x[0] : " << initial_x1 << "->" << x_Quadratic[0] << endl;
    cout << "x[1] : " << initial_x2 << "->" << x_Quadratic[1] << endl;
    cout << "x[2] : " << initial_x3 << "->" << x_Quadratic[2] << endl;
    cout << "x[3] : " << initial_x4 << "->" << x_Quadratic[3] << endl;

    return 0;
}

#endif